/*(C) 2011 Hannes Hassler. 
May be used under 
GNU GENERAL PUBLIC LICENSE (GPL3)
http://www.gnu.org/licenses/gpl-3.0.html
For different licencense ask
hannes.hassler@gmail.com
*/

#include <SensorAktor.h>
#include <SoftwareServo.h>

#define NoNl     false
#define Nl       true


#include "Wire.h"
#include <Helios.h>
#include <DS1107.h>



SoftwareServo azimuth;
SoftwareServo elevation;
int winkel;
int winkelAzimuth;
int winkelElevation;


Helios helios;
DS1107 clock;


double dAzimuth;
double dElevation;
      
char ch;

//to set the timer chip edit these constants, compile and
//upload it with s (you can rejustify hour (with d) and minute (with f))
byte second=0;
byte minute=45;
byte hour=8;
byte dayOfWeek=6;
byte dayOfMonth=21;
byte month=5;
byte year=11;


//Köln coordinates
float localLongitude=6.9599;
float localLatitude=50.9406;


byte currentAzPos=0;
byte newAzPos=1;
byte currentElvPos=0;
byte newElvPos=1;



//Constants for KHM ServoShield
float k_az=0.5;
float k_elv=0.5;

/*Starting Positions d_az and d_elv
Must be updated according to Heliostat Target.
Adjust manually with a and e in the Serial Monitor
*/
int d_az=41;
int d_elv=130;


void setup()
{
  
   
    pinMode(servo1, OUTPUT); //azimuth
    pinMode(servo2, OUTPUT); //elevation
    azimuth.attach(servo1);
    elevation.attach(servo2);
    winkel = 0;
    winkelAzimuth = 1;
    winkelElevation = 1; 
  
    Serial.begin(9600);  
  
  
}


void loop()
{

 static int v = 0; 

 if (Serial.available()) { 
      ch = Serial.read();  
      switch(ch) {
     case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'E':
        show("elevation val:",v,Nl); 
        elevation.write(v);               
        v = 0; break;
      case 'A':
        show("azimuthVal:",v,Nl);
        azimuth.write(v);
        v = 0; break;      
      case 'c':   
       clock.setDate(second,minute,hour,dayOfWeek,dayOfMonth,month,year);
       break;
      case 'd':
       show("set hour:", v, Nl);
       hour=v;
       v=0;break;
      case 'f':
       show("set minute:", v, Nl);
       minute=v;
       v=0;break;
      case 'a':
       show("set d_az:",v,Nl);
       d_az=v; 
       v=0; break;
      case 'e':
       show("set d_elv:",v,Nl);
       d_elv=v;
       v=0; break;
      case 'p':        
        clock.getDate();  
        helios.calcSunPos(2000+clock.year,clock.month,clock.dayOfMonth,
        clock.hour, clock.minute,clock.second,localLongitude,localLatitude); 
        showTime(clock);
        dAzimuth=helios.dAzimuth;show("dAzimuth",dAzimuth,true);
        dElevation=helios.dElevation;show("dElevation",dElevation,true);
        show("d_az:",d_az,Nl);
        show("d_elv:",d_elv,Nl);
       break;   
        
    }
  }  
  
  positionHeliostatKHM();
  SoftwareServo::refresh();
  
}



void show(char nameStr[], double val, boolean newline) {
  Serial.print(nameStr);  
  Serial.print("=");
  if (newline)
       Serial.println(val);
  else Serial.print(val);
}

void showTime(DS1107 timerChip) {
  Serial.print("UT ");
  Serial.print(timerChip.hour, DEC);
  Serial.print(":");
  Serial.print(timerChip.minute, DEC);
  Serial.print(":");
  Serial.print(timerChip.second, DEC);
  Serial.print("  ");
  Serial.print(timerChip.month, DEC);
  Serial.print("/");
  Serial.print(timerChip.dayOfMonth, DEC);
  Serial.print("/");
  Serial.print(timerChip.year, DEC);
  Serial.print("  Day_of_week:");
  Serial.println(timerChip.dayOfWeek, DEC);  
}


void calcCurrentAzimuthElevation() {
  
   clock.getDate();  
   helios.calcSunPos(2000+clock.year,clock.month,clock.dayOfMonth,
   clock.hour, clock.minute,clock.second,localLongitude,localLatitude); 
   
   dAzimuth=helios.dAzimuth;
   dElevation=helios.dElevation;
   
   newAzPos=(k_az*dAzimuth)+d_az;
   newElvPos=(k_elv*dElevation)+d_elv;
  
    
}

void positionHeliostatKHM() {
  
  
    calcCurrentAzimuthElevation();
    
   if (newAzPos!=currentAzPos) {
            showTime(clock);
            show("newAzPos:",newAzPos,Nl);  
            currentAzPos=newAzPos;
            azimuth.write(newAzPos);           
            
   }
        
   if (newElvPos!=currentElvPos) {
            showTime(clock);
            show("newElvPos:",newElvPos,Nl);
            currentElvPos=newElvPos;
            elevation.write(newElvPos);            
  }
  
  
}



